%作业6_9

initial_state = zeros(2,3);
a_dj.reset_state(initial_state);

% 响应越快，误差曲线会越接近速度轨迹的形状
enable_m = repmat([1 1 0 0 0],2,1);
div_feedforward_con = doubleJointControler(a_dj,tor_motor,0.01,1,enable_m);
div_feedforward_con.pid_reset();


%%
%注意数据存储方式，是组号(关节1/2)- 时间 - 数据类型（加速度速度或者位置）
motor_res = zeros(2,length(1:simu_step),3);
state_info = zeros(2,length(1:simu_step),3);
debug_info = zeros(2,length(1:simu_step),4);


%%

%先把pid架设好
con_value =[0;0];
calculated_torque = [0;0];
sample_time_count = sample_period;
for i = 1:simu_step
    if(sample_time_count >= sample_period)
        tmp = repmat(pos_s_traj_value(i,:),2,1);
        div_feedforward_con.set_target(tmp);
        debug_info(:,i,:) = div_feedforward_con.get_id_con_value(a_dj,tor_motor,simu_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    motor_res(:,i,:) = tor_motor.simu_loop(a_dj,simu_period,con_value);
    state_info(:,i,:) = (a_dj.simu_loop(motor_res(:,i,1),simu_period));
    sample_time_count = sample_time_count+simu_period;
end
%%

figure;
yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
yyaxis right
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1)-state_info(1,:,1)','b:o','MarkerIndices',1:1000:simu_step,'MarkerSize',2);
hold on
% plot(0:sample_period:(simu_step-1)*simu_period,debug_info(2,1:(sample_period/simu_period):end-1,2),'y:o','MarkerIndices',1:100:(simu_step)/(sample_period/simu_period),'MarkerSize',2);
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1)-state_info(2,:,1)',':o','MarkerIndices',1:1000:simu_step,'MarkerSize',2);

legend('joint 1','joint 2','target','joint 1 err','joint 2 err','Location',"southeast");
title("pos S traj follow  theta");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('joint 1','joint 2','target');
title("pos S traj follow  omega");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,3),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% ylim([-1 1]);
legend('joint 1','joint 2','target');
title("pos S traj follow  acc");